#include "maindefs.h"
#include <usart.h>
#include "my_uart.h"

static uart_comm *uc_ptr;

/*
 * Returns the size of a loconet message with the specified opcode.
 * The size is including the opcode and checksum.
 * Returns -1 if unrecognized opcode, 0 if variable length or the actual
 * length if a recognized opcode and set length.
 */
signed char message_size(unsigned char opc) {
	unsigned char msg_type = 0xF0 & opc;
	signed char retval = -1;
	switch(msg_type) {
	case 0x80:
		retval = 2;
		break;
	case 0xB0:
		retval = 4;
		break;
	case 0xA0:
		retval = 4;
		break;
	case 0xE0:
		retval = 0; // variable size message
		break;
	}
	return retval;
}

/*
 *  Returns 1 if the byte is an opcode, 0 if not
 */
unsigned char is_opcode(unsigned char byte) {
	// the most significant bit is the opcode flag
	return (byte >> 7);
}

/*
 *  Returns 1 if the checksum is valid, 0 if not.
 *  Note length is the full length of the message including the checksum
 */
unsigned char is_valid_checksum(unsigned char cksum, char *msg, unsigned char length) {
	unsigned char checker = msg[0];
	unsigned char i = 1;
	// XOR all the data bytes together
	for(; i < length-1; i++) {
		checker ^= msg[i];
	}
	// One's complement
	checker = ~checker;
	return (checker == cksum);
}

void uart_recv_int_handler()
{
	signed char retval = 0;
#if LOCONET_PIC
	UARTLED = !UARTLED;
#endif

	if (DataRdyUSART()) {
		unsigned char cur_byte = ReadUSART();
#if LOCONET_PIC
		// New loconet message?
		if(is_opcode(cur_byte) == 1) {
			// Start of a new message
			uc_ptr->buflen = 1;
			uc_ptr->buffer[0] = cur_byte;
		}
		else if(uc_ptr->buflen == 0) {
			// Invalid byte, may have been in the middle of a message
			// before we received the opcode move onto the next byte
			// Reset values
			HANDLE_ERROR(-9);
			return;
		}
		else {
			// In the middle of a message
			signed char full_len = message_size(uc_ptr->buffer[0]);

			if(full_len == 0) {
				// Variable length message
				if(uc_ptr->buflen == 1) {
					// At the length byte
					full_len = cur_byte;
				}
				else if(uc_ptr->buflen > 1) {
					// Length byte already stored, retrieve it
					full_len = uc_ptr->buffer[1];
				}
			}
			else if(full_len < 0) {
				// Error, an incorrect opcode was provided
				HANDLE_ERROR(-5);
				return;
			}

			if(uc_ptr->buflen < full_len - 1) {
				// This byte is an argument, store it
				uc_ptr->buffer[uc_ptr->buflen] = cur_byte;
				// Increment length
				uc_ptr->buflen++;
			}
			else if(uc_ptr->buflen == full_len - 1) {
				// This byte is a checksum, check it
				unsigned char checksum = cur_byte;
				if(is_valid_checksum(checksum,uc_ptr->buffer,full_len)) {
					// If the checksum is valid, we have the full message
					uc_ptr->buffer[uc_ptr->buflen] = checksum;
					uc_ptr->buflen++;
				}
				else {
					uc_ptr->buflen = 0;
				}
			}
			else {
				// Shouldn't get here
				HANDLE_ERROR(-6);
				return;
			}

			if(uc_ptr->buflen > 14) {
				// Verify that the buflen is not too long for the queue
				HANDLE_ERROR(uc_ptr->buflen);
				return;
			}
			// Check if a LocoNet message is ready to be sent to I2C
			if(uc_ptr->buflen == full_len) {
				// Send current loconet message to I2C
//				LATAbits.LATA1 = 1;
				retval = ToMainHigh_sendmsg(uc_ptr->buflen, MSGT_UART_DATA, (void *) uc_ptr->buffer);
				if(retval < 0) {
					HANDLE_ERROR(retval);
				}
				// Set length back to 0
				uc_ptr->buflen = 0;
			}
		}
#else
		uc_ptr->buffer[uc_ptr->buflen] = cur_byte;
		uc_ptr->buflen++;
		// check if a message should be sent
		if (uc_ptr->buflen == MAXUARTBUF) {
			retval = ToMainLow_sendmsg(uc_ptr->buflen, MSGT_UART_DATA, (void *) uc_ptr->buffer);
			uc_ptr->buflen = 0;
		} else if (uc_ptr->buflen > MAXUARTBUF) {
			// Major problem, why is this happening?
			HANDLE_ERROR(uc_ptr->buflen);
		}
		if (retval < 0) {
			// if we get an error, call the error handler
			HANDLE_ERROR(retval);
		}
#endif
	}
	if (USART_Status.OVERRUN_ERROR == 1) {
		// we've overrun the USART and must reset
		// send an error message for this
		RCSTAbits.CREN = 0;
		RCSTAbits.CREN = 1;
		HANDLE_ERROR(-10);
	}

}

void init_uart_recv(uart_comm *uc)
{
	uc_ptr = uc;
	uc_ptr->buflen = 0;
}
